package robocup.component.geometry;

import java.util.LinkedList;

import robocup.component.geometry.analysers.ReachableArea;

/**
 * This class models an straight line. A straight line <code>l</code> is
 * represented by an equation of the form: y = mx + n. So the StraightLine has
 * no start point and end point.
 */
public class StraightLine {

  /**
   * The gradient.
   */
  private double              _m;
  /**
   * The intersection with the y axis.
   */
  private double              _n;

  /** ************************** DUMMIES ************************* */

  private final Vektor        dummyVektor = Vektor.getDummyInstance();

  /** ************************** STATICS ************************* */

  /**
   * "epsilon"
   */
  private final static double E           = 0.000001;

  /** ************************** CONSTRUCTORS ********************* */

  /**
   * Constructs a new straight line.
   * 
   * <br>
   * NEW OBJECTS: 1 + this
   * 
   * @param m
   *          The gradient. m=0 means parallel to middle line of the field. m=
   *          POSITIVE_INFINITY means parallel to the side lines (then n ==
   *          x-coordinate)
   * @param n
   *          The intersection with the y axis.
   */
  public StraightLine(
      final double m,
      final double n) {

    if (m == Double.NEGATIVE_INFINITY) {
      this._m = Double.POSITIVE_INFINITY;
    }
    else {
      this._m= m;
    }
    this._n = n;
  }

  /**
   * Constructs a new straight line with 2 vektors.
   * 
   * <br>
   * NEW OBJECTS: 1+this
   * 
   * @param p
   *          first vektor.
   * @param q
   *          second vektor.
   */
  public StraightLine(
      final Vektor p,
      final Vektor q) {

    // avoid division by zero
    final double m;
    final double n;
    if (p.x == q.x) {
      m = Double.POSITIVE_INFINITY;
      n = p.x;
    }
    else {
      m = (q.y - p.y) / (q.x - p.x);
      n = q.y - m * q.x;
    }

    this._m = m;
    this._n = n;
  }

  /**
   * Constructs a new straight line through <code>point</code> with a slope
   * <code>angle</code> in degree.
   * 
   * <br>
   * NEW OBJECTS: 1+this
   * 
   * @param point
   * @param angle
   */
  public StraightLine(
      final Vektor point,
      double angle) {

    if (angle == 0) {
      this._m = Double.POSITIVE_INFINITY;
      this._n = point.x;
    }
    else {
      this._m = 1 / Math.tan(Math.toRadians(angle));
      this._n = point.y - this._m * point.x;
    }
  }

  /** ************************** STATICS ************************** */

  /**
   * Creates a new StraightLine.
   * 
   * <br>
   * NEW OBJECTS: 2
   * 
   * @return a StraightLine
   */
  public static final StraightLine getDummyInstance() {

    return new StraightLine(0, 0);
  }

  /**
   * Calculates if the three given points are on one StraightLine
   * 
   * @param p1
   *          point1
   * @param p2
   *          point2
   * @param p3
   *          point3
   * 
   * @return true if the three points are on the same line
   */
  public static final boolean onOneLine(final Vektor p1,
      final Vektor p2,
      final Vektor p3) {

    if (p1.equals(p2)) {
      return true;
    }
    if (p1.x == p2.x) {
      if (p1.x + StraightLine.E >= p3.x && p1.x - StraightLine.E <= p3.x) {
        return true;
      }
      return false;
    }
    double lambda = (p3.x - p1.x) / (p2.x - p1.x);
    return p3.y <= (p1.y + lambda * (p2.y - p1.y)) + StraightLine.E
        && p3.y >= (p1.y + lambda * (p2.y - p1.y)) - StraightLine.E;
  }

  /** ************************** METHODS ************************** */

  /**
   * Sets the StraightLine
   * 
   * @param m
   *          The gradient. m=0 means parallel to middle line of the field.<br>
   *          m=POSITIVE_INFINITY means parallel to the side lines (in this
   *          case, n is the x-value)
   * @param n
   *          The intersection with the y axis.
   */
  public void setStraightLine(final double m,
      final double n) {

    this._m = m == Double.NEGATIVE_INFINITY ? Double.POSITIVE_INFINITY : m;
    this._n = n;
  }

  /**
   * Sets the StraightLine through <code>point</code> with a slope
   * <code>angle</code>.
   * 
   * @param point
   * @param angle
   */
  public void setStraightLine(final Vektor point,
      final double angle) {

    if (angle == 0) {
      this._m = Double.POSITIVE_INFINITY;
      this._n = point.x;
    }
    else {
      this._m = 1 / Math.tan(Math.toRadians(angle));
      this._n = point.y - this._m * point.x;
    }
  }

  /**
   * sets the StraightLine by 2 vektors.
   * 
   * @param p
   *          first vektor.
   * @param q
   *          second vektor.
   */
  public void setStraightLine(final Vektor p,
      final Vektor q) {

    // avoid division by zero
    final double m;
    final double n;
    if (p.x == q.x) {
      m = Double.POSITIVE_INFINITY;
      n = p.x;
    }
    else {
      m = (q.y - p.y) / (q.x - p.x);
      n = q.y - m * q.x;
    }

    this._m = m;
    this._n = n;
  }

  /**
   * @return Returns m.
   */
  public final double getM() {

    return this._m;
  }

  /**
   * @return Returns n.
   */
  public final double getN() {

    return this._n;
  }

  /**
   * The slope of a line in degree.
   * 
   * @return the slope
   */
  public final double getSlope() {

    if (_m == 0) {
      return 90;
    }
    if (_m == Double.POSITIVE_INFINITY) {
      return 0;
    }

    return Math.toDegrees(Math.atan(1 / this._m));
  }

  /**
   * Copies the values of line into this object
   * 
   * @param line
   *          the StraightLine to copy
   */
  public void copy(final StraightLine line) {

    _m = line._m;
    _n = line._n;
  }

  /**
   * Creates a duplicate of this line
   * 
   * <br>
   * NEW OBJECTS: 2
   * 
   * @return a duplicate of this object
   */
  public final StraightLine cloned() {

    return new StraightLine(this._m, this._n);
  }

  /**
   * This method calculates the shortest Vektor from a given point to a line.
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param point
   * @param result -
   *          here the result is saved (overwrites old contents)
   */
  public void getVektorToLine(final Vektor point,
      final Vektor result) {

    // special case: m=0
    if (this._m == 0) {
      result.x = 0;
      result.y = this._n - point.y;
      return;
    }
    if (_m == Double.POSITIVE_INFINITY) {
      result.y = 0;
      result.x = this._n - point.x;
      return;
    }

    final double n = point.x / this._m + point.y;
    // find the intersection point of the two lines
    this.getIntersectionPoint(-1 / this._m, n, result);
    result.subFromThis(point);
  }

  /**
   * This method calculates the shortest Vektor from a given point to a line.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param point
   * @return the shortest Vektor from a given point to a line.
   */
  public final Vektor getVektorToLine(final Vektor point) {

    Vektor res = new Vektor();
    getVektorToLine(point, res);
    return res;
  }

  /**
   * This method calculates this line's distance to a given point.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param point
   *          The point.
   * @return The distance.
   */
  public final double getDistanceToPoint(final Vektor point) {

    return this.getVektorToLine(point).getLength();
  }

  /**
   * This method checks if the given Vektor is on line
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param point
   * @return true if the point is on the line, false otherwise
   */
  public final boolean isOnLine(final Vektor point) {

    if (_m == Double.POSITIVE_INFINITY) {
      if (point.x + StraightLine.E >= _n && point.x - StraightLine.E <= _n) {
        return true;
      }
    }
    if ((point.y <= this._m * point.x + this._n + StraightLine.E)
        && (point.y >= this._m * point.x + this._n - StraightLine.E)) {
      return true;
    }
    return false;
  }

  /**
   * This method calculates the intersection of this line and another line given
   * by m and n.
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param m
   *          the gradiant of the second line<br>
   *          m=0 means parallel to middle line m=POSITIVE_INFINITY means
   *          parallel to goalo-to-goal-line (in this case n should be x-value
   *          of the line)
   * @param n
   *          the intersection point with the y-axis of the second line.
   * @param result
   *          here the result is saved (overwrites old contents)
   */
  public void getIntersectionPoint(final double m,
      final double n,
      final Vektor result) {

    // if the lines are parallel, i don't feel like searching for the
    // intersection point...
    if (this._m + StraightLine.E >= m && this._m - StraightLine.E <= m) {
      result.setEmpty();
      return;
    }
    if (this._m == Double.POSITIVE_INFINITY) {
      result.x = this._n;
      result.y = m * this._n + n;
      return;
    }
    if (m == Double.POSITIVE_INFINITY || m == Double.NEGATIVE_INFINITY) {
      result.x = n;
      result.y = this._m * n + this._n;
      return;
    }
    if (m == 0) {
      result.y = n;
      result.x = (n - this._n) / this._m;
      return;
    }
    double mNew = this._m;
    double nNew = this._n;
    double intersectionPointX, intersectionPointY;
    nNew -= n;
    mNew /= m;
    nNew /= m;
    intersectionPointX = nNew / (1 - mNew);
    intersectionPointY = this._m * intersectionPointX + this._n;
    result.x = intersectionPointX;
    result.y = intersectionPointY;
  }

  /**
   * This method calculates the intersection of this line and another line given
   * by m and n.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param m
   *          the gradiant of the second line
   * @param n
   *          the intersection point with the y-axis of the second line.
   * @return the intersection point, or null if the lines are parallel
   */
  public final Vektor getIntersectionPoint(final double m,
      final double n) {

    Vektor res = new Vektor();
    getIntersectionPoint(m, n, res);
    return res.isEmpty() ? null : res;
  }

  /**
   * Calculates the intersection point between this StraightLine and a LineSegment and adds it to the list.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the LineSegment to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointToList(final LineSegment line,
      final LinkedList<Vektor> list) {
    line.addIntersectionPointToList(this, list);
  }

  /**
   * Calculates the intersection points between this StraightLine and a CircleSegment and adds them to the list.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param circ
   *          the CircleSegment to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointsToList(final CircleSegment circ,
      final LinkedList<Vektor> list) {
    circ.addIntersectionPointsToList(this, list);
  }

  /**
   * Calculates the intersection points between this StraightLine and a Circle and adds them to the list.
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param circ
   *          the Circle to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointsToList(final Circle circ,
      final LinkedList<Vektor> list) {
    circ.addIntersectionPointsToList(this, list);
  }

  /**
   * Calculates the intersection point between two StraightLines and adds them to the list.
   * 
   * <br>
   * NEW OBJECTS:1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param line
   *          the StraightLine to intersect with
   * @param list
   *          here the result will be added
   */
  public void addIntersectionPointToList(final StraightLine line,
      final LinkedList<Vektor> list) {
    Vektor v= getIntersectionPoint(line);
    if (v != null) {
      list.add(v);
    }
  }

  /**
   * This method calculates the intersection point. If both lines are parallel,
   * the result is set to empty.
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param line
   *          the given line.
   * @param result -
   *          here the result is saved (overwrites old contents)
   * 
   */
  public void getIntersectionPoint(final StraightLine line,
      final Vektor result) {

    getIntersectionPoint(line._m, line._n, result);
  }

  /**
   * This method calculates the intersection of this line and another given
   * line.
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param line
   *          the given line.
   * @return The intersection point, or null if both lines are parallel.
   */
  public final Vektor getIntersectionPoint(final StraightLine line) {

    Vektor res = new Vektor();
    getIntersectionPoint(line._m, line._n, res);
    return res.isEmpty() ? null : res;
  }

  /**
   * This method calculates the intersection of this line and a LineSegment
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the given line.
   * @return The intersection point, or null if both lines are parallel.
   */
  public final Vektor getIntersectionPoint(final LineSegment line) {

    return line.getIntersectionPoint(this);
  }

  /**
   * This method calculates the intersection point. If both lines are parallel,
   * the result is set to empty.
   * 
   * <br>
   * USES LineSegment.dummyVektor
   * 
   * @param line
   *          the given line.
   * @param result -
   *          here the result is saved (overwrites old contents)
   * 
   */
  public void getIntersectionPoint(final LineSegment line,
      final Vektor result) {

    line.getIntersectionPoint(this, result);
  }

  /**
   * Calculates the intersection points of this line with the given Circle
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param c
   *          the Circle to intersect with
   * @return a LinkedList containing 0-2 Vektors
   */
  public final LinkedList<Vektor> getIntersectionPoints(final Circle c) {

    LinkedList<Vektor> list = new LinkedList<Vektor>();
    getIntersectionPoints(c, list);
    return list;
  }

  /**
   * Calculates the intersection points of this line with the given Circle
   * 
   * <br>
   * NEW OBJECTS <= 2
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param c
   *          the Circle to intersect with
   * @param result
   *          a LinkedList. Its contents will be overwritten with 0-2 Vektors.
   */
  public void getIntersectionPoints(final Circle c,
      final LinkedList<Vektor> result) {

    if (!result.isEmpty()) {
      result.clear();
    }
    double dist = getDistanceToPoint(c.getCenter());
    if (dist > c.getRadius()) {
      return;
    }
    if (dist == c.getRadius()) {
      getVektorToLine(c.getCenter(), dummyVektor);
      dummyVektor.addToThis(c.getCenter());
      result.add(dummyVektor.cloned());
      return;
    }
    getVektorToLine(c.getCenter(), dummyVektor);
    dummyVektor.addToThis(c.getCenter());
    double kath = Math.sqrt(c.getRadius() * c.getRadius() - dist * dist);
    Vektor p1 = getDirection().multThis(kath);
    p1.addToThis(dummyVektor);
    result.add(p1);
    Vektor p2 = getDirection().multThis(-kath);
    p2.addToThis(dummyVektor);
    result.add(p2);
    return;
  }

  /**
   * Calculates the intersection points of this line with the given
   * CircleSegment
   * 
   * <br>
   * NEW OBJECTS <= 3
   * 
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param c
   *          the CircleSegment to intersect with
   * @return a LinkedList containing 0-2 Vektors
   */
  public final LinkedList<Vektor> getIntersectionPoints(final CircleSegment c) {

    LinkedList<Vektor> list = new LinkedList<Vektor>();
    getIntersectionPoints(c, list);
    return list;
  }

  /**
   * Calculates the intersection points of this line with the given Circle
   * 
   * <br>
   * NEW OBJECTS <= 2
   * <br>
   * USES StraightLine.dummyVektor
   * 
   * @param c
   *          the CircleSegment to intersect with
   * @param result
   *          a LinkedList. Its contents will be overwritten with 0-2 Vektors.
   */
  public void getIntersectionPoints(final CircleSegment c,
      final LinkedList<Vektor> result) {

    if (!result.isEmpty()) {
      result.clear();
    }
    double dist = getDistanceToPoint(c.getCenter());
    if (dist > c.getRadius()) {
      return;
    }
    if (dist == c.getRadius()) {
      getVektorToLine(c.getCenter(), dummyVektor);
      dummyVektor.addToThis(c.getCenter());
      if (c.includesAngle(c.getCenter().getAngleTo(dummyVektor))) {
        result.add(dummyVektor.cloned());
      }
      return;
    }
    getVektorToLine(c.getCenter(), dummyVektor);
    dummyVektor.addToThis(c.getCenter());
    double kath = Math.sqrt(c.getRadius() * c.getRadius() - dist * dist);
    Vektor p1 = getDirection().multThis(kath);
    p1.addToThis(dummyVektor);
    if (c.includesAngle(c.getCenter().getAngleTo(p1))) {
      result.add(p1);
    }
    Vektor p2 = getDirection().multThis(-kath);
    p2.addToThis(dummyVektor);
    if (c.includesAngle(c.getCenter().getAngleTo(p2))) {
      result.add(p2);
    }
    return;
  }
  
  /**
   * Calculates the intersection points of this StraightLine and a RA and add the results to the given list.
   * 
   * @param ra the RA to intersect with
   * @param back if true, backwards movement is allowed
   * @return a list that contains the intersection points
   */
  public final LinkedList<Vektor> getIntersectionPoints(final ReachableArea ra, final boolean back) {
    return ra.getIntersectionPoints(this, back);
  }
  
  /**
   * Calculates the intersection points of this StraightLine and a RA and add the results to the given list.
   * 
   * @param ra the RA to intersect with
   * @param back if true, backwards movement is allowed
   * @param list here the results are added
   */
  public final void addIntersectionPointsToList(final ReachableArea ra, final boolean back, final LinkedList<Vektor> list) {
    ra.addIntersectionPointsToList(this, back, list);
  }

  /**
   * Calculates the direction of this StraightLine
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @return a vektor of length 1 and with the direction of this line
   */
  public final Vektor getDirection() {

    Vektor result = new Vektor();
    getDirection(result);
    return result;
  }

  /**
   * Calculates the direction of this StraightLine
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param result
   *          the contents is overwritten with a Vaktor of length 1 and the
   *          direction of the line.
   */
  public void getDirection(final Vektor result) {

    result.setVektor(1, getSlope());
  }
  
  /**
   * Calculates the point on the line with the given y-value
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param yValue the needed y-value
   * @return the point with the given yValue
   */
  public final Vektor getPointWithYValue(final double yValue) {
    Vektor res= new Vektor();
    getPointWithYValue(yValue, res);
    return res.isEmpty()?null:res;
  }

  /**
   * Calculates the point on the line with the given y-value
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param yValue the needed y-value
   * @param result here the result is saved. empty if there is no such point. old contents will be overwritten
   */
  public void getPointWithYValue(final double yValue,
      final Vektor result) {
    if (_m== 0) {
      result.setEmpty();
      return;
    }
    if (_m == Double.POSITIVE_INFINITY) {
      result.x= _n;
      result.y= yValue;
      return;
    }
    result.x= (yValue - _n)/ _m;
    result.y= yValue;
  }
  
  /**
   * Calculates the point on the line with the given x-value
   * 
   * <br>
   * NEW OBJECTS: 1
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param xValue the needed x-value
   * @return the point with the given xValue
   */
  public final Vektor getPointWithXValue(final double xValue) {
    Vektor res= new Vektor();
    getPointWithYValue(xValue, res);
    return res.isEmpty()?null:res;
  }

  /**
   * Calculates the point on the line with the given x-value
   * 
   * <br>
   * USES --NO DUMMIES--
   * 
   * @param xValue the needed x-value
   * @param result here the result is saved. empty if there is no such point. old contents will be overwritten
   */
  public void getPointWithXValue(final double xValue,
      final Vektor result) {
    if (_m== 0) {
      result.x= xValue;
      result.y= _n;
      return;
    }
    if (_m == Double.POSITIVE_INFINITY) {
      result.setEmpty();
      return;
    }
    result.x= xValue;
    result.y= xValue*_m+_n;
  }

  /**
   * Returns a string representation of this line.
   * 
   * @return a String representation of the StraightLine
   */
  @Override
  public final String toString() {
    
    if (_m == Double.POSITIVE_INFINITY) {
      return "x = "+_n+" | ";
    }

    String line = "-";
    if (this._m < 0) {
      line = "\\";
    }
    if (this._m > 0) {
      line = "/";
    }
    final String sign = this._n < 0 ? "" : "+ ";
    return "y = " + this._m + " * x " + sign + this._n + " " + line;
  }
}
